#include "cavp.h"
#include <math.h>
#include <stdlib.h>

DynaDiff::DynaDiff(double lowOutFreq, int sampleFreq)
{
	m_intvTime = 1.0 / sampleFreq;
	for (int i = 0; i < 5; i++) {
		m_posiBuffer[i] = 0.0;
	}

	m_inSmooth = new LowPass(100, sampleFreq);
	m_outPass = new LowPass(lowOutFreq, sampleFreq);
}

DynaDiff::~DynaDiff()
{
	delete m_outPass;
	delete m_inSmooth;
}

void DynaDiff::Reset(void)
{
	for (int i = 0; i < 5; i++) {
		m_posiBuffer[i] = 0.0;
	}
}

void DynaDiff::WriteOnTick(double posi)
{
	m_inSmooth->WriteOnTick(posi);
	if (m_inSmooth->DataReadyOnTick()) {
		for (int i = 4; i > 0; i--) {
			m_posiBuffer[i] = m_posiBuffer[i - 1];
		}
		m_posiBuffer[0] = m_inSmooth->Read();

		double velo = (21 * m_posiBuffer[0] - 32 * m_posiBuffer[1] + 12 * m_posiBuffer[2] - m_posiBuffer[4]) / 12 / m_intvTime;
		m_outPass->WriteOnTick(velo);
	}
}

BOOL DynaDiff::DataReadyOnTick(void)
{
	return(m_outPass->DataReadyOnTick());
}

double DynaDiff::Read(void)
{
	return(m_outPass->Read());
}

AccePosiSynth::AccePosiSynth(double cutoffFreq, int sampleFreq)
{
	m_intvTime = 1.0 / sampleFreq;
	m_acceInte = 0.0;
	m_highPass = new HighPass(cutoffFreq, sampleFreq);
	m_posiDiff = new DynaDiff(cutoffFreq, sampleFreq);
}

AccePosiSynth::~AccePosiSynth()
{
	delete m_highPass;
	delete m_posiDiff;
}

void AccePosiSynth::Reset(void)
{
	m_posiDiff->Reset();
	m_acceInte = 0.0;
}

void AccePosiSynth::Write2OnTick(double acce, double posi)
{
	m_acceInte += acce * m_intvTime;
	m_highPass->WriteOnTick(m_acceInte);
	m_posiDiff->WriteOnTick(posi);
}

BOOL AccePosiSynth::DataReadyOnTick(void)
{
	return(m_highPass->DataReadyOnTick() & m_posiDiff->DataReadyOnTick());
}

double AccePosiSynth::Read(void)
{
	return(m_highPass->Read() + m_posiDiff->Read());
}

AcceSynth::AcceSynth(double fv, double fp, int sampleFreq)
{
	m_fv = fv;
	m_fp = fp;
	m_intvTime = 1.0 / sampleFreq;
	m_acceInte = 0.0;
	m_veloInte = 0.0;
}

AcceSynth::~AcceSynth()
{
}

void AcceSynth::Reset(void)
{
	m_acceInte = 0.0;
	m_veloInte = 0.0;
}

void AcceSynth::WriteOnTick(double acce)
{
	double foreAcce = acce - m_fv * m_acceInte - m_fp * m_veloInte;
	m_acceInte += foreAcce * m_intvTime;
	m_veloInte += m_acceInte * m_intvTime;
}
